#include "outline/approximate.h"
#include "outline/type_traits.h"
#include "glib/g2/smatrix.h"
#include "glib/g2/algorithm.h"
#include <limits>

namespace outline {

const double double_max = std::numeric_limits<double>::max();

///last_segment[i] describes the last approximation contour's cut for 
///the original subcountor comprised from nodes 0...i
template <typename R>
struct segment {
	typedef g2::basic_vector<R> vector_r;
	double d; ///< metrics of approximation discrepancy for the entire contour with end here (-1, if contour's end is possible here)
	unsigned first_node; ///< # of first node from original contour that is approximated by this last segment (0, if this segment is the first one)
	vector_r centre; ///< a (center) point on the segment
	vector_r dir; ///< vector (of any length) pointing direction of the segment
	vector_r junction; ///< intersection point with previous segment
public:
	point<R> projection(vector_r const & p) const {
		vector_r res = centre + (scalar_product(p - centre, dir) / sqr(dir)) * dir;
		return point<R>(res.x, res.y);
	}
	point<R> junction_point() const
		{ return point<R>(junction.x, junction.y); }
	///\return true if two segments have admissible intersection
	///\param common_point is a point that both segments have to approximate
	template <typename T>
	bool intersection(segment<R> const & b, point<T> const & common_point) 	{
		segment<R> const & a = * this;
		junction = common_point;
		if (outer_product(a.dir, b.dir) == 0)
			return outer_product(a.dir, a.centre - b.centre) == 0;
		g2::basic_vector<R> res;
		res = intersect1(a.centre, a.dir, b.centre, b.dir);
		g2::basic_vector<R> com = common_point;
		if (scalar_product(res - a.centre, a.dir)*scalar_product(com - a.centre, a.dir) <= 0)
			return false;
		if (scalar_product(res - b.centre, b.dir)*scalar_product(com - b.centre, b.dir) <= 0)
			return false;
		junction = res;
		return true;
	}
};

///The class accumulates information points of original contour to be approximated by a line,
///and provides parameters of best line passing through the centre mass of original points,
///as well as discrepancy of original points from the best line.
template <typename T>
class best_centre_mass_line {
public:
	typedef T elem_type;

	typedef typename type_traits<T>::accumulator_type accumulator_type;
	typedef g2::basic_vector<accumulator_type>	vector;
	typedef g2::basic_smatrix<accumulator_type>	smatrix;

	typedef typename type_traits<T>::real_type real_type;
	typedef g2::basic_vector<real_type>	vector_r;
public:
	best_centre_mass_line()
		{ clear(); }
public:
	///forgets accumulated information and prepares to approximate new set of points
	void clear()
		{ N = 0; S.zero(); Q.zero(); }
	///accumulated information about else one point from the set
	void add(vector const & p) {
		++N;
		S += p;
		Q += tsquare(p);
	}
	///Euclidean discrepancy of original points from the best line
	double discrepancy() const {
		g2::smatrix q = this->q();
		return q.tr() - q.eigenvalue(1);
	}
	///returns centre mass of accumulated point set
	vector_r centre() const
		{ return vector_r(S) * real_type(1.0 / N); }
	///returns vector (of any length) pointing direction of the best approximating line
	vector_r dir() const
		{ return q().eigenvector(1); }
	///faster combination of 2 functions
	double discrepancy_n_dir(vector_r & dir) const {
		g2::smatrix q = this->q();
		double eigenvalue;
		dir = q.eigenvector(1, false, &eigenvalue);
		return q.tr() - eigenvalue;
	}
private:
	g2::smatrix q() const
		{ return g2::smatrix(Q) - (1.0 / N) * tsquare(g2::vector(S)); }
private:
	int N; ///< total count of accumulated points
	vector S; ///< total vector sum of accumulated points' positions
	smatrix Q; ///< total sum of tensor squares of accumulated points' positions
};

template <typename T, typename U>
static double approximate_min_step_len_core(int n, ///< min_step_len
	std::vector< point<T> > const & src, std::vector< point<U> > & dst, bool closed)
{
	assert(n > 1);

	size_t const m = src.size();
	std::vector< segment<U> > ns(m);

	ns[0].d = -1;

	size_t i;
	best_centre_mass_line<T> line;
	for (i = 0; i < n && i < m; ++i) {
		line.add(src[i]);
		ns[i].d = -1;
		ns[i].first_node = 0; //for contours of small length
	}
	for (; i < 2*n && i < m; ++i) {
		line.add(src[i]);
		ns[i].d = line.discrepancy_n_dir(ns[i].dir);
		ns[i].centre = line.centre();
		ns[i].first_node = 0;
	}
	for (; i < m; ++i) {
		size_t max_j = (i < 3*n-1) ? i-n+1 : 2*n;
		line.clear();
		ns[i].d = double_max;
		segment<U> bad; //bad solution used in the case when there is no good one - hack :(
		bad.d = double_max;
		size_t j;
		for (j = 0; j < n; ++j)
			line.add(src[i-j]);
		for (; j < max_j; ++j) {
			line.add(src[i-j]);
			assert(ns[i-j].d >= 0);
			segment<U> test;
			test.first_node = i - j;
			test.centre = line.centre();
			test.d = ns[i-j].d + line.discrepancy_n_dir(test.dir);
			if (!test.intersection(ns[i-j], src[i-j])) {
				if (bad.d > test.d)
					bad = test;
				continue;
			}
			if (test.d < ns[i].d) {
				ns[i] = test;

				//test for better start
				if (closed && j == 2*n-1) {
					int s = i - j;
					int c = 1;
					for (int k = 1; k < n; ++k)
						if (ns[i-k].first_node == s)
							++c;
						else
							break;
					if (c == n) {
						//we found better fixed start point for closed curve
						ns.clear();
						std::vector< point<T> > tmp;
						shift(src, tmp, s);
						return approximate_min_step_len_core(n, tmp, dst, 
							false //no closed flag because second start reselection is unwanted
						);
					}
				}
			}
		}
		if (ns[i].d == double_max) {
			assert(bad.d != double_max);
			ns[i] = bad;
		}
	}

	//back pass
	std::vector< point<U> > tmp;
	tmp.push_back(ns[m-1].projection(src[m-1]));
	int seg1 = m-1;
	int seg2;
	for (;;) {
		segment<U> & last = ns[seg1];
		if (last.first_node == 0)
			break;
		seg2 = last.first_node;
		tmp.push_back(last.junction_point());
		seg1 = seg2;
	}
	tmp.push_back(ns[seg1].projection(src[0]));

	if (src[0] == src[m-1]) {
		ns[m-1].intersection(ns[seg1], src[0]); //ns[m-1] is damaged here
		tmp[0] = tmp.back() = ns[m-1].junction_point();
	}

	dst.assign(tmp.rbegin(), tmp.rend());

	return ns[m-1].d;
}

template <typename T, typename U>
double approximate_min_step_len(int min_step_len,
	std::vector< point<T> > const & src, std::vector< point<U> > & dst)
{
	if (min_step_len <= 1) {
		if ((void*)&src != (void*)&dst)
			dst.assign(src.begin(), src.end());
		return 0;
	}

	return approximate_min_step_len_core(min_step_len, src, dst, closed(src));
}

template <typename T, typename U>
double approximate_min_step_len(int min_step_len,
	std::vector<std::vector<point<T> > > const & src, std::vector<std::vector<point<U> > > & dst)
{
	dst.resize(src.size());
	double s = 0;
	for (int i = 0; i < src.size(); ++i)
		s += approximate_min_step_len(min_step_len, src[i], dst[i]);
	return s;
}

template double approximate_min_step_len(int min_step_len,
	outlines_us const & src, outlines_f & dst);
template double approximate_min_step_len(int min_step_len,
	outlines_s const & src, outlines_f & dst);
template double approximate_min_step_len(int min_step_len,
	outlines_i const & src, outlines_f & dst);
template double approximate_min_step_len(int min_step_len,
	outlines_f const & src, outlines_f & dst);
template double approximate_min_step_len(int min_step_len,
	outlines_d const & src, outlines_d & dst);

//

template <typename T, typename U>
static void best_segment_ending_here(segment<U> & res, std::vector< point<T> > const & src,
									 int end, std::vector< segment<U> > const & layer)
{
	best_centre_mass_line<T> line;
	line.add(src[end]);
	res.d = double_max;
	segment<U> bad; //bad solution used in the case when there is no good one - hack :(
	bad.d = double_max;
	for (int j = end-1; layer[j].d >= 0; --j) {
		line.add(src[j]);
		segment<U> test;
		test.first_node = j;
		test.centre = line.centre();
		double ld = line.discrepancy_n_dir(test.dir);
		test.d = layer[j].d + ld;
		if (!test.intersection(layer[j], src[j])) {
			if (bad.d > test.d)
				bad = test;
			continue;
		}
		if (test.d < res.d)
			res = test;
		else if (ld > res.d)
			break;
	}
	if (res.d == double_max) {
		assert(bad.d != double_max);
		res = bad;
	}
}

template <typename T, typename U>
static void make_first_layer(std::vector< point<T> > const & src,
	std::vector< segment<U> > & first)
{
	first[0].d = -1;
	size_t m = first.size();
	best_centre_mass_line<T> line;
	line.clear();
	line.add(src[0]);
	for (int i = 1; i < m; ++i) { //no distinction between open and closed contours
		line.add(src[i]);
		first[i].d = line.discrepancy_n_dir(first[i].dir);
		assert(first[i].d >= 0);
		first[i].first_node = 0;
		first[i].centre = line.centre();
	}
}

template <typename T, typename U>
static void make_next_layer(std::vector< point<T> > const & src,
	std::vector< segment<U> > const & last, std::vector< segment<U> > & next)
{
	int i;

	size_t m = last.size();
	next.resize(m);
	next[0].d = -1;
	for (i = 1; last[i-1].d < 0 && i < m; ++i)
		next[i].d = -1;
	for (; i < m; ++i)
		best_segment_ending_here(next[i], src, i, last);
}

///builds optimal coutour in reverse order by passing through layers
template <typename T, typename U>
static void layers_back_pass(std::vector< point<T> > const & src,
	std::vector< std::vector< segment<U> > > const & layers, segment<U> & last,
	std::vector< point<U> > & reverse)
{
	size_t m = layers.back().size();
	reverse.push_back(last.projection(src[m-1]));
	segment<U> const * seg1 = &last;
	int i2 = last.first_node;
	int k = layers.size() - 1;
	while (i2 != 0) {
		reverse.push_back(seg1->junction_point());
		segment<U> const * seg2 = &layers[k][i2];
		i2 = seg2->first_node;
		--k;
		seg1 = seg2;
	}
	assert(k == -1);
	reverse.push_back(seg1->projection(src[0]));

	if (closed(src)) {
		last.intersection(*seg1, src[0]); //last is damaged here
		reverse[0] = reverse.back() = last.junction_point();
	}
}

template <typename T, typename U>
static double approximate_to_K_nodes_core(int K,
	std::vector< point<T> > const & src, std::vector< point<U> > & dst, bool closed)
{ 
	assert (K >= 2);
	size_t const m = src.size();
	assert (m >= 2);

	int i, k;
	best_centre_mass_line<T> line;

	if (K == 2 || K == 3 && closed) {
		std::vector< point<U> > tmp(2);
		if (closed) {
			tmp[0] = src[0];
			tmp[1] = src[m-1];
			dst.swap(tmp);
			return 0;
		}
		else {
			//solution is known, we have to calculate discrepancy only
			for (i = 0; i < m; ++i)
				line.add(src[i]);
			segment<U> test;
			test.centre = line.centre();
			test.dir = line.dir();
			tmp[0] = test.projection(src[0]);
			tmp[1] = test.projection(src[m-1]);
			dst.swap(tmp);
			return line.discrepancy();
		}
	}

	typedef std::vector< segment<U> > nodes;
	std::vector<nodes> layers(K-2);

	layers[0].resize(m);
	make_first_layer(src, layers[0]);

	for (k = 1; k < K-2; ++k) 
		make_next_layer(src, layers[k-1], layers[k]);

	//last segment
	segment<U> last;
	best_segment_ending_here(last, src, m-1, layers[K-3]);

	if (closed && K >= 3) {
		//how many times node is leved in contour,
		//summed for simplified contours of size 3..K
		std::vector<int> stat(m);
		for (int j = 1; j <= K-3; ++j) {
			if (j == K-3)
				i = last.first_node;
			else
				i = layers[j+1][m-1].first_node;
			k = j;
			while (i != 0) {
				++stat[i];
				i = layers[k][i].first_node;
				--k;
			}
			assert(k == -1);
		}
		int s = std::max_element(stat.begin(), stat.end()) - stat.begin();

		//new fixed start point for closed curve is choosen
		stat.clear();
		layers.clear();
		std::vector< point<T> > tmp;
		shift(src, tmp, s);
		return approximate_to_K_nodes_core(K, tmp, dst, 
			false //no closed flag because second start reselection is unwanted
		);
	}

	std::vector< point<U> > tmp;
	layers_back_pass(src, layers, last, tmp);
	dst.assign(tmp.rbegin(), tmp.rend());

	return last.d;
}

template <typename T, typename U>
double approximate_to_K_nodes(int K,
	std::vector< point<T> > const & src, std::vector< point<U> > & dst)
{
	if (K <= 1) {
		if ((void*)&src != (void*)&dst)
			dst.assign(src.begin(), src.end());
		return 0;
	}

	return approximate_to_K_nodes_core(K, src, dst, closed(src));
}


template <typename T, typename U>
double approximate_avg_step_len(float avg_step_len,
	std::vector<std::vector<point<T> > > const & src, std::vector<std::vector<point<U> > > & dst)
{
	dst.resize(src.size());
	double s = 0;
	for (int i = 0; i < src.size(); ++i) {
		int K = src[i].size() / avg_step_len + 0.5f;
		if (K < 2)
			K = 2;
		s += approximate_to_K_nodes(K, src[i], dst[i]);
	}
	return s;
}

template double approximate_avg_step_len(float avg_step_len,
	outlines_us const & src, outlines_f & dst);
template double approximate_avg_step_len(float avg_step_len,
	outlines_s const & src, outlines_f & dst);
template double approximate_avg_step_len(float avg_step_len,
	outlines_i const & src, outlines_f & dst);
template double approximate_avg_step_len(float avg_step_len,
	outlines_f const & src, outlines_f & dst);
template double approximate_avg_step_len(float avg_step_len,
	outlines_d const & src, outlines_d & dst);

//

template <typename T, typename U>
static double approximate_max_discrepancy_core(double max_discrepancy,
	std::vector< point<T> > const & src, std::vector< point<U> > & dst, bool closed)
{
	assert(max_discrepancy > 0);
	size_t const m = src.size();
	assert (m >= 2);

	best_centre_mass_line<T> line;

	typedef std::vector< segment<U> > nodes;
	std::vector<nodes> layers(1);
	layers.reserve(m); //no resizing is permitted

	layers[0].resize(m);
	make_first_layer(src, layers[0]);

	int k;
	for (k = 0; layers[k][m-1].d > max_discrepancy; ++k) {
		layers.push_back(nodes());
		make_next_layer(src, layers[k], layers[k+1]);
	}

	if (k == 0) {
		std::vector< point<U> > tmp(2);
		tmp[0] = src[0];
		tmp[1] = src[m-1];
		dst.swap(tmp);
		return layers[k][m-1].d;
	}

	if (closed && k >= 1) {
		//how many times node is leved in contour,
		//summed for simplified contours of arbitrary size
		std::vector<int> stat(m);
		for (int j = 1; j <= k; ++j) {
			int i = layers[j][m-1].first_node;
			int l = j-1;
			while (i != 0) {
				++stat[i];
				i = layers[l][i].first_node;
				--l;
			}
			assert(l == -1);
		}
		int s = std::max_element(stat.begin(), stat.end()) - stat.begin();

		//new fixed start point for closed curve is choosen
		stat.clear();
		layers.clear();
		std::vector< point<T> > tmp;
		shift(src, tmp, s);
		return approximate_max_discrepancy_core(max_discrepancy, tmp, dst, 
			false //no closed flag because second start reselection is unwanted
		);
	}

	segment<U> last = layers[k][m-1];
	//remove last layer
	layers.resize(layers.size()-1);

	std::vector< point<U> > tmp;
	layers_back_pass(src, layers, last, tmp);
	dst.assign(tmp.rbegin(), tmp.rend());

	return last.d;
}

template <typename T, typename U>
double approximate_max_discrepancy(double max_discrepancy,
	std::vector< point<T> > const & src, std::vector< point<U> > & dst)
{
	if (max_discrepancy == 0) {
		if ((void*)&src != (void*)&dst)
			dst.assign(src.begin(), src.end());
		return 0;
	}
	return approximate_max_discrepancy_core(max_discrepancy, src, dst, closed(src));
}

template <typename T, typename U>
double approximate_avg_discrepancy(double avg_discrepancy,
	std::vector<std::vector<point<T> > > const & src, std::vector<std::vector<point<U> > > & dst)
{
	dst.resize(src.size());
	double s = 0;
	for (int i = 0; i < src.size(); ++i)
		s += approximate_max_discrepancy(src[i].size() * avg_discrepancy, src[i], dst[i]);
	return s;
}

template double approximate_avg_discrepancy(double avg_discrepancy,
	outlines_us const & src, outlines_f & dst);
template double approximate_avg_discrepancy(double avg_discrepancy,
	outlines_s const & src, outlines_f & dst);
template double approximate_avg_discrepancy(double avg_discrepancy,
	outlines_i const & src, outlines_f & dst);
template double approximate_avg_discrepancy(double avg_discrepancy,
	outlines_f const & src, outlines_f & dst);
template double approximate_avg_discrepancy(double avg_discrepancy,
	outlines_d const & src, outlines_d & dst);

} //namespace outline
